/* LinkMatrices.java
 *
 * Handles computing matrices for the robot, using forward kinematics to find link locations
 */

import Jama.Matrix;

public class LinkMatrices {
    private Matrix t01, t12, t23, t34;
    private Matrix t0toEnd, t0to3, t0to2;
    
    private double rotation1, rotation2;
    private Line link1, link2, link3;

    public LinkMatrices(Line ln1, Line ln2, Line ln3) {
        link1 = ln1; link2 = ln2; link3 = ln3;
        setSlider(ln1.getStartX());
        setJ1(Math.PI);
        setJ2(0);
        setEnd();
        recalculate();
    }

    /*
     * Set matrix for sliding joint
     */
    private void setSlider(double d) {
	    double[][] array1 = {{Math.cos(0),-Math.sin(0),0,0},
                   {Math.sin(0)*Math.cos(0),Math.cos(0)*Math.cos(0),-Math.sin(0),d*-Math.sin(0)},
                   {Math.sin(0)*Math.sin(0),Math.cos(0)*Math.sin(0),Math.cos(0),Math.cos(0)*d},
                   {0,0,0,1}};
        t01 = new Matrix(array1);
    }

    /*
     * Set matrix for first rotating joint
     */
    private void setJ1(double theta) {
        double[][] array2 = {{Math.cos(theta),-Math.sin(theta),0,350},
                   {Math.sin(theta)*Math.cos(-Math.PI/2),Math.cos(theta)*Math.cos(-Math.PI/2),-Math.sin(-Math.PI/2),0*-Math.sin(-Math.PI/2)},
                   {Math.sin(theta)*Math.sin(-Math.PI/2),Math.cos(theta)*Math.sin(-Math.PI/2),Math.cos(-Math.PI/2),Math.cos(-Math.PI/2)*0},
                   {0,0,0,1}};
        t12 = new Matrix(array2);
        rotation1 = theta;
    }

    /*
     * Set matrix for second rotating joint
     */
    private void setJ2(double theta) {
        double[][] array3 = {{Math.cos(theta),-Math.sin(theta),0,100},
                   {Math.sin(theta)*Math.cos(0),Math.cos(theta)*Math.cos(0),-Math.sin(0),0*-Math.sin(0)},
                   {Math.sin(theta)*Math.sin(0),Math.cos(theta)*Math.sin(0),Math.cos(0),Math.cos(0)*0},
                   {0,0,0,1}};
        t23 = new Matrix(array3);
        rotation2 = theta;
    }

    /*
     * Set matrix for end effector
     */
    private void setEnd() {
        double[][] array4 = {{Math.cos(0),-Math.sin(0),0,75},
                   {Math.sin(0)*Math.cos(0),Math.cos(0)*Math.cos(0),-Math.sin(0),0*-Math.sin(0)},
                   {Math.sin(0)*Math.sin(0),Math.cos(0)*Math.sin(0),Math.cos(0),Math.cos(0)*0},
                   {0,0,0,1}};
        t34 = new Matrix(array4);
    }

    /*
     * Calculates the matrices and updates the coordinates.
     */
    private void recalculate() {
        t0to2 = t01.times(t12);
        t0to3 = t0to2.times(t23);
        t0toEnd = t0to3.times(t34);
        // print the matrices
        //t01.print(5,3);
        //t0to2.print(5,3);
        //t0to3.print(5,3);
        //t0toEnd.print(5,3);

        double[][] coords = {{0}, {0}, {0}, {1}};
        // link1
        Matrix tmp = new Matrix(coords);
        Matrix tmp2 = t01.times(tmp);
        link1.setStartX((int)tmp2.get(2, 0));
        link1.setEndX((int)tmp2.get(2, 0));

        // link2
        tmp2 = t0to3.times(tmp);
        link2.setStartX(link1.getEndX());
        link2.setStartY(link1.getEndY());
        link2.setEndX((int)tmp2.get(2, 0));
        link2.setEndY((int)tmp2.get(0, 0));

        // link3
        tmp2 = t0toEnd.times(tmp);
        link3.setStartX(link2.getEndX());
        link3.setStartY(link2.getEndY());
        link3.setEndX((int)tmp2.get(2, 0));
        link3.setEndY((int)tmp2.get(0, 0));
    }

    /*
     * Slide left (negative) or right (positive) by d
     */
    public void slide(double d) {
        setSlider(link1.getStartX() + d);
        recalculate();
    }

    public void slideTotal(double d) {
        setSlider(d);
        recalculate();
    }

    /*
     * Rotate link 2 by theta (radians)
     */
    public void rot2(double theta) {
        setJ1(rotation1 + theta);
        recalculate();
    }
    
    public void rot2Total(double theta) {
        setJ1(theta);
        recalculate();
    }
    
    /*
     * Rotate link 3 by theta (radians)
     */
    public void rot3(double theta) {
        setJ2(rotation2 + theta);
        recalculate();
    }

    public void rot3Total(double theta) {
        setJ2(theta);
        recalculate();
    }
    
    /*
     * Set up the robot using inverse kinematics using the given endpoint
     */
    public void inverse_kinematics(double z_end, double x_end)
    {
    	
    	//each link length
    	double L1 = 150;
    	double L2 = 100;
    	double L3 = 75;
    	
    	//System.out.println("z_end = "+z_end+" x_end = "+x_end);
    
    	//Current end effector
    	double z_cur = link3.getEndX()-175;
    	double x_cur = 500-link3.getEndY();
    	
    	//System.out.println("current z: "+z_cur+"current x: "+x_cur);
    	
    	if( (z_end-z_cur) != 0 )
    	{
    		//Need to move the entire robot over by the difference
    		double d = z_end-z_cur;
    		slide(d);
    	}
    
    	if((x_end-x_cur)!= 0)
    	{
    		if(x_end<=325&&x_end>0){
            //System.out.println(x_end-x_cur);
    		//Need to solve triangle 
    		
    		double c = Math.sqrt(Math.pow((z_end - (link1.getEndX()-175)),2)+Math.pow(x_end - ((500-link1.getEndY())), 2));
    		//System.out.println("c = "+c);
    		
    		//Law of cosines
    		double v = (Math.pow(L3, 2)-Math.pow(L2, 2)-Math.pow(c, 2))/(-2*L2*c);
    		//System.out.println("V = "+v);
    		
    		double move= 0;
    		if(Math.abs(v)>1){
    			if(c>=175)
                    slide(-25);
    			else
    				slide(25);
    			c = Math.sqrt(Math.pow((z_end - (link1.getEndX()-175)),2)+Math.pow(x_end - ((500-link1.getEndY())), 2));
    		}	
    		
    		double c2 = Math.sqrt(Math.pow((z_end - (link1.getStartX()-175)),2)+Math.pow(x_end - ((500-link1.getStartY())), 2));

            v = (Math.pow(L3, 2)-Math.pow(L2, 2)-Math.pow(c, 2))/(-2*L2*c);
            double v1= (Math.pow(c2, 2)-Math.pow(L1, 2)-Math.pow(c, 2))/(-2*L1*c);
            double theta1a = Math.acos(v);
            double theta1 = Math.acos(v1)+theta1a;
            //System.out.println("theta1 = "+ theta1);
            //theta1 = theta1;
            
            double v2 = (Math.pow(c, 2)-Math.pow(L2, 2)-Math.pow(L3, 2))/(-2*L2*L3);
            //System.out.println("V2 = "+v2);
            
            if(Math.abs(v2)<1){
                double theta2 = Math.acos(v2);
                //System.out.println("theta2 = "+ theta2);
    
                //Perform total rotations
                rot2Total(theta1);
                rot3Total(-(Math.PI-theta2));
    		}
    	}
    		//System.out.println();
    		//System.out.println();
    		
    	}
    	
    }
    
    
    public Line getLink1() {
        return link1;
    }

    public Line getLink2() {
        return link2;
    }

    public Line getLink3() {
        return link3;
    }
}
